#include "Rectify.h"
#include "cv.h"
#include "OpencvFuncMacro.h"

#pragma warning( disable: 4244)

namespace NS_RECTIFICATION
{
	//implementation of [1]
	void Rectify_Simple(/*IN*/const float* Rot0, /*IN*/ const float* Trans0, 
		/*IN*/const float* Intri0, 
		/*IN*/const float* Rot1, /*IN*/ const float* Trans1,
		/*IN*/const float* Intri1,
		/*OUT*/float* P0, /*OUT*/float* T0, 
		/*OUT*/float* P1, /*OUT*/float* T1)
	{
		//old intrinsic matrix
		CvMat* intriMat0 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* intriMat1 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( intriMat0 && intriMat1 );

		//old rotation matrix
		CvMat* rotMat0 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* rotMat1 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( rotMat0 && rotMat1 );

		//old translation
		CvMat* transMat0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* transMat1 = cvCreateMat( 3, 1, CV_32FC1 );
		assert( transMat0 && transMat1);
		
		//center
		CvMat* Center0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* Center1 = cvCreateMat( 3, 1, CV_32FC1 );
		assert( Center0 && Center1 );

		//new Axes
		CvMat* XAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* YAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* ZAxis = cvCreateMat( 3, 1, CV_32FC1 );
		assert( XAxis && YAxis && ZAxis );

		//new Rotation
		CvMat* RotNew = cvCreateMat( 3, 3, CV_32FC1 );
		assert( RotNew );
		//new intrinsic matrix
		CvMat* IntrinsicTmp = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* IntrinsicNew= cvCreateMat( 3, 3, CV_32FC1 );
		assert( IntrinsicNew && IntrinsicTmp);

		//new Projection Matrix 
		CvMat* ProjNew0 = cvCreateMat( 3, 4, CV_32FC1 );
		CvMat* ProjNew1 = cvCreateMat( 3, 4, CV_32FC1 );
		assert( ProjNew0 && ProjNew1 );

		//Transformation for each view
		CvMat* TransformMat0 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* TransformMat1 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( TransformMat0 && TransformMat1 );

		//tmp matrix used in this function
		CvMat* tmpMat0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* tmpMat1 = cvCreateMat( 1, 3, CV_32FC1 );
		CvMat* tmpMat3 = cvCreateMat( 3, 4, CV_32FC1 );
		assert( tmpMat0 && tmpMat1 && tmpMat3 );
		
		int i, j;
		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				CV_MAT_ELEM( *rotMat0, float, i, j ) = Rot0[i*3+j];
				CV_MAT_ELEM( *rotMat1, float, i, j ) = Rot1[i*3+j];
				CV_MAT_ELEM( *intriMat0, float, i, j ) = Intri0[i*3+j];
				CV_MAT_ELEM( *intriMat1, float, i, j ) = Intri1[i*3+j];
			}
			CV_MAT_ELEM( *transMat0, float, i, 0 ) = Trans0[i];
			CV_MAT_ELEM( *transMat1, float, i, 0 ) = Trans1[i];
		}

		//C=-R^t*T
		cvGEMM( rotMat0, transMat0, -1.0, NULL, 0.0, 
			Center0, CV_GEMM_A_T );
		cvGEMM( rotMat1, transMat1, -1.0, NULL, 0.0, 
			Center1, CV_GEMM_A_T );

		//new X Axis (=direction of the baseline)
		cvSub( Center0, Center1, XAxis );
		//new Y Axis (orthogonal to new x and old z )
		cvGetRow( rotMat0, tmpMat1, 2 );
		cvTranspose( tmpMat1, tmpMat0 );
		cvCrossProduct( tmpMat0, XAxis, YAxis);
		//new Z Axis (orthogonal to new x and new y )
		cvCrossProduct( XAxis, YAxis, ZAxis);

		float fn;
		//new Rotation Matrix
		fn = sqrt( cvNorm( XAxis ) );
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 0, j ) = CV_MAT_ELEM( *XAxis, float, j, 0 )/fn;
		}

		fn = sqrt( cvNorm( YAxis ));
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 1, j ) = CV_MAT_ELEM( *YAxis, float, j, 0 )/fn;
		}

		fn = sqrt( cvNorm( ZAxis ));
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 2, j ) = CV_MAT_ELEM( *ZAxis, float, j, 0 )/fn;
		}

		//new intrinsic matrix
		cvAdd( intriMat0, intriMat1, IntrinsicTmp );
		cvScale( IntrinsicTmp, IntrinsicNew, 0.5 );
		CV_MAT_ELEM( *IntrinsicNew, float, 0, 1 ) = 0.0f;

		//new projection matrix
		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				CV_MAT_ELEM( *tmpMat3, float, i, j ) = CV_MAT_ELEM( *RotNew, float, i, j );
			}
		}

		cvGEMM( RotNew, Center0, -1.0, NULL, 0.0, 
			transMat0, 0 );

		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *transMat0, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *transMat0, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *transMat0, float, 2, 0 );

		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew0, 0);

		cvGEMM( RotNew, Center1, -1.0, NULL, 0.0, 
			transMat1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *transMat1, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *transMat1, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *transMat1, float, 2, 0 );
		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew1, 0);

		//rectifying image transformation
		CvMat* tmpMat4 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* tmpMat5 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( tmpMat4 && tmpMat5 );

		//transform0
		cvGEMM( intriMat0, rotMat0, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		cvGetCols( ProjNew0, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat0, 0);

		//transform1
		cvGEMM( intriMat1, rotMat1, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		cvGetCols( ProjNew1, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat1, 0);


		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				T0[i*3+j]=CV_MAT_ELEM( *TransformMat0, float, i, j );
				T1[i*3+j]=CV_MAT_ELEM( *TransformMat1, float, i, j );
			}
		}

		for( i=0; i<3; ++i )
		{
			for( j=0; j<4; ++j )
			{
				P0[i*4+j]=CV_MAT_ELEM( *ProjNew0, float, i, j );
				P1[i*4+j]=CV_MAT_ELEM( *ProjNew1, float, i, j );
			}
		}


		CV_SAFE_RELEASE_MAT( intriMat0 );
		CV_SAFE_RELEASE_MAT( intriMat1 );
		CV_SAFE_RELEASE_MAT( rotMat0 );
		CV_SAFE_RELEASE_MAT( rotMat1 );
		CV_SAFE_RELEASE_MAT( transMat0 );
		CV_SAFE_RELEASE_MAT( transMat1 );
		CV_SAFE_RELEASE_MAT( Center0 );
		CV_SAFE_RELEASE_MAT( Center1 );
		CV_SAFE_RELEASE_MAT( XAxis );
		CV_SAFE_RELEASE_MAT( YAxis );
		CV_SAFE_RELEASE_MAT( ZAxis );
		CV_SAFE_RELEASE_MAT( RotNew );
		CV_SAFE_RELEASE_MAT( IntrinsicTmp );
		CV_SAFE_RELEASE_MAT( IntrinsicNew );
		CV_SAFE_RELEASE_MAT( ProjNew0 );
		CV_SAFE_RELEASE_MAT( ProjNew1 );
		CV_SAFE_RELEASE_MAT( TransformMat0 );
		CV_SAFE_RELEASE_MAT( TransformMat1 );
		CV_SAFE_RELEASE_MAT( tmpMat0 );
		CV_SAFE_RELEASE_MAT( tmpMat1 );
		CV_SAFE_RELEASE_MAT( tmpMat3 );
		CV_SAFE_RELEASE_MAT( tmpMat4 );
		CV_SAFE_RELEASE_MAT( tmpMat5 );

	}

	void Rectify_Simple( /*IN*/const CvMat* rotMat0, /*IN*/ const CvMat* transMat0, 
		/*IN*/const CvMat* intriMat0, 
		/*IN*/const CvMat* rotMat1, /*IN*/ const CvMat* transMat1,
		/*IN*/const CvMat* intriMat1,
		/*OUT*/CvMat* ProjNew0, /*OUT*/CvMat* TransformMat0, 
		/*OUT*/CvMat* ProjNew1, /*OUT*/CvMat* TransformMat1)
	{
		int i, j;

		//old intrinsic matrix
		assert( intriMat0->rows==3 && intriMat1->rows==3
			&& intriMat0->cols==3 && intriMat1->cols==3 );

		//old rotation matrix
		assert( rotMat0->rows==3 && rotMat1->rows==3
			&& rotMat0->cols==3 && rotMat1->cols==3 );
		
		//old translation
		assert( transMat0->rows==3 && transMat1->rows==3
			&& transMat0->cols==1 && transMat1->cols==1 );


		//center
		CvMat* Center0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* Center1 = cvCreateMat( 3, 1, CV_32FC1 );
		assert( Center0 && Center1 );

		//new Axes
		CvMat* XAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* YAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* ZAxis = cvCreateMat( 3, 1, CV_32FC1 );
		assert( XAxis && YAxis && ZAxis );

		//new Rotation
		CvMat* RotNew = cvCreateMat( 3, 3, CV_32FC1 );
		assert( RotNew );
		//new intrinsic matrix
		CvMat* IntrinsicTmp = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* IntrinsicNew= cvCreateMat( 3, 3, CV_32FC1 );
		assert( IntrinsicNew && IntrinsicTmp);

		//new Projection Matrix 
		assert( ProjNew0->rows==3 && ProjNew1->rows==3
			&& ProjNew0->cols==4 && ProjNew1->cols==4 );

		//Transformation for each view
		assert( TransformMat0->rows==3 && TransformMat1->rows==3
			&& TransformMat0->cols==4 && TransformMat1->cols==4 );

		//tmp matrix used in this function
		CvMat* tmpMat0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* tmpMat1 = cvCreateMat( 1, 3, CV_32FC1 );
		CvMat* tmpMat3 = cvCreateMat( 3, 4, CV_32FC1 );
		assert( tmpMat0 && tmpMat1 && tmpMat3 );



		//C=-R^t*T
		cvGEMM( rotMat0, transMat0, -1.0, NULL, 0.0, 
			Center0, CV_GEMM_A_T );
		cvGEMM( rotMat1, transMat1, -1.0, NULL, 0.0, 
			Center1, CV_GEMM_A_T );

		//new X Axis (=direction of the baseline)
		cvSub( Center0, Center1, XAxis );
		//new Y Axis (orthogonal to new x and old z )
		cvGetRow( rotMat0, tmpMat1, 2 );
		cvTranspose( tmpMat1, tmpMat0 );
		cvCrossProduct( tmpMat0, XAxis, YAxis);
		//new Z Axis (orthogonal to new x and new y )
		cvCrossProduct( XAxis, YAxis, ZAxis);

		float fn;
		//new Rotation Matrix
		fn = sqrt( cvNorm( XAxis ) );
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 0, j ) = CV_MAT_ELEM( *XAxis, float, j, 0 )/fn;
		}

		fn = sqrt( cvNorm( YAxis ));
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 1, j ) = CV_MAT_ELEM( *YAxis, float, j, 0 )/fn;
		}

		fn = sqrt( cvNorm( ZAxis ));
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 2, j ) = CV_MAT_ELEM( *ZAxis, float, j, 0 )/fn;
		}

		//new intrinsic matrix
		cvAdd( intriMat0, intriMat1, IntrinsicTmp );
		cvScale( IntrinsicTmp, IntrinsicNew, 0.5 );
		CV_MAT_ELEM( *IntrinsicNew, float, 0, 1 ) = 0.0f;

		//new projection matrix
		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				CV_MAT_ELEM( *tmpMat3, float, i, j ) = CV_MAT_ELEM( *RotNew, float, i, j );
			}
		}

		cvGEMM( RotNew, Center0, -1.0, NULL, 0.0, 
			tmpMat0, 0 );

		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 2, 0 );

		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew0, 0);

		cvGEMM( RotNew, Center1, -1.0, NULL, 0.0, 
			tmpMat0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *tmpMat0, float, 2, 0 );
		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew1, 0);

		//rectifying image transformation
		CvMat* tmpMat4 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* tmpMat5 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( tmpMat4 && tmpMat5 );

		//transform0
		cvGEMM( intriMat0, rotMat0, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		cvGetCols( ProjNew0, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat0, 0);

		//transform1
		cvGEMM( intriMat1, rotMat1, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		cvGetCols( ProjNew1, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat1, 0);

		CV_SAFE_RELEASE_MAT( Center0 );
		CV_SAFE_RELEASE_MAT( Center1 );
		CV_SAFE_RELEASE_MAT( XAxis );
		CV_SAFE_RELEASE_MAT( YAxis );
		CV_SAFE_RELEASE_MAT( ZAxis );
		CV_SAFE_RELEASE_MAT( RotNew );
		CV_SAFE_RELEASE_MAT( IntrinsicTmp );
		CV_SAFE_RELEASE_MAT( IntrinsicNew );
		CV_SAFE_RELEASE_MAT( tmpMat0 );
		CV_SAFE_RELEASE_MAT( tmpMat1 );
		CV_SAFE_RELEASE_MAT( tmpMat3 );
		CV_SAFE_RELEASE_MAT( tmpMat4 );
		CV_SAFE_RELEASE_MAT( tmpMat5 );

	}


	void Rectify_Simple( /*IN*/const float* Rot0, /*IN*/ const float* Trans0, 
		/*IN*/const float* Intri0, 
		/*IN*/const float* Rot1, /*IN*/ const float* Trans1,
		/*IN*/const float* Intri1,
		/*OUT*/CvMat* ProjNew0, /*OUT*/CvMat* TransformMat0, 
		/*OUT*/CvMat* ProjNew1, /*OUT*/CvMat* TransformMat1)
	{
		//old intrinsic matrix
		CvMat* intriMat0 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* intriMat1 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( intriMat0 && intriMat1 );

		//old rotation matrix
		CvMat* rotMat0 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* rotMat1 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( rotMat0 && rotMat1 );

		//old translation
		CvMat* transMat0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* transMat1 = cvCreateMat( 3, 1, CV_32FC1 );
		assert( transMat0 && transMat1);
		
		//center
		CvMat* Center0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* Center1 = cvCreateMat( 3, 1, CV_32FC1 );
		assert( Center0 && Center1 );

		//new Axes
		CvMat* XAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* YAxis = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* ZAxis = cvCreateMat( 3, 1, CV_32FC1 );
		assert( XAxis && YAxis && ZAxis );

		//new Rotation
		CvMat* RotNew = cvCreateMat( 3, 3, CV_32FC1 );
		assert( RotNew );
		//new intrinsic matrix
		CvMat* IntrinsicTmp = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* IntrinsicNew= cvCreateMat( 3, 3, CV_32FC1 );
		assert( IntrinsicNew && IntrinsicTmp);


		//tmp matrix used in this function
		CvMat* tmpMat0 = cvCreateMat( 3, 1, CV_32FC1 );
		CvMat* tmpMat1 = cvCreateMat( 1, 3, CV_32FC1 );
		CvMat* tmpMat3 = cvCreateMat( 3, 4, CV_32FC1 );
		assert( tmpMat0 && tmpMat1 && tmpMat3 );
		
		int i, j;
		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				CV_MAT_ELEM( *rotMat0, float, i, j ) = Rot0[i*3+j];
				CV_MAT_ELEM( *rotMat1, float, i, j ) = Rot1[i*3+j];
				CV_MAT_ELEM( *intriMat0, float, i, j ) = Intri0[i*3+j];
				CV_MAT_ELEM( *intriMat1, float, i, j ) = Intri1[i*3+j];
			}
			CV_MAT_ELEM( *transMat0, float, i, 0 ) = Trans0[i];
			CV_MAT_ELEM( *transMat1, float, i, 0 ) = Trans1[i];
		}

		//C=-R^t*T
		cvGEMM( rotMat0, transMat0, -1.0, NULL, 0.0, 
			Center0, CV_GEMM_A_T );
		cvGEMM( rotMat1, transMat1, -1.0, NULL, 0.0, 
			Center1, CV_GEMM_A_T );
		
		//new X Axis (=direction of the baseline)
		cvSub( Center0, Center1, XAxis );
		//new Y Axis (orthogonal to new x and old z )
		cvGetRow( rotMat0, tmpMat1, 2 );
		cvTranspose( tmpMat1, tmpMat0 );
		cvCrossProduct( tmpMat0, XAxis, YAxis);
		//new Z Axis (orthogonal to new x and new y )
		cvCrossProduct( XAxis, YAxis, ZAxis);

		float fn;
		//new Rotation Matrix
		fn = cvNorm( XAxis );
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 0, j ) = CV_MAT_ELEM( *XAxis, float, j, 0 )/fn;
		}

		fn = cvNorm( YAxis );
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 1, j ) = CV_MAT_ELEM( *YAxis, float, j, 0 )/fn;
		}

		fn = cvNorm( ZAxis );
		for( j=0; j<3; ++j )
		{
			CV_MAT_ELEM( *RotNew, float, 2, j ) = CV_MAT_ELEM( *ZAxis, float, j, 0 )/fn;
		}

		//new intrinsic matrix
		cvAdd( intriMat0, intriMat1, IntrinsicTmp );
		cvScale( IntrinsicTmp, IntrinsicNew, 0.5 );
		CV_MAT_ELEM( *IntrinsicNew, float, 0, 1 ) = 0.0f;

		//new projection matrix
		for( i=0; i<3; ++i )
		{
			for( j=0; j<3; ++j )
			{
				CV_MAT_ELEM( *tmpMat3, float, i, j ) = CV_MAT_ELEM( *RotNew, float, i, j );
			}
		}

		cvGEMM( RotNew, Center0, -1.0, NULL, 0.0, 
			transMat0, 0 );

		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *transMat0, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *transMat0, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *transMat0, float, 2, 0 );

		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew0, 0);
		cvGEMM( RotNew, Center1, -1.0, NULL, 0.0, 
			transMat1, 0 );

		CV_MAT_ELEM( *tmpMat3, float, 0, 3 ) = CV_MAT_ELEM( *transMat1, float, 0, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 1, 3 ) = CV_MAT_ELEM( *transMat1, float, 1, 0 );
		CV_MAT_ELEM( *tmpMat3, float, 2, 3 ) = CV_MAT_ELEM( *transMat1, float, 2, 0 );
		cvGEMM( IntrinsicNew, tmpMat3, 1.0, NULL, 0.0, ProjNew1, 0);

		//rectifying image transformation
		CvMat* tmpMat4 = cvCreateMat( 3, 3, CV_32FC1 );
		CvMat* tmpMat5 = cvCreateMat( 3, 3, CV_32FC1 );
		assert( tmpMat4 && tmpMat5 );

		//transform0
		cvGEMM( intriMat0, rotMat0, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		
		cvGetCols( ProjNew0, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat0, 0);

		//transform1
		cvGEMM( intriMat1, rotMat1, 1.0, NULL, 0.0, tmpMat4, 0);
		cvInvert( tmpMat4, tmpMat5 );
		cvGetCols( ProjNew1, tmpMat4, 0, 3 );
		cvGEMM( tmpMat4, tmpMat5, 1.0, NULL, 0.0, TransformMat1, 0);


		CV_SAFE_RELEASE_MAT( intriMat0 );
		CV_SAFE_RELEASE_MAT( intriMat1 );
		CV_SAFE_RELEASE_MAT( rotMat0 );
		CV_SAFE_RELEASE_MAT( rotMat1 );
		CV_SAFE_RELEASE_MAT( transMat0 );
		CV_SAFE_RELEASE_MAT( transMat1 );
		CV_SAFE_RELEASE_MAT( Center0 );
		CV_SAFE_RELEASE_MAT( Center1 );
		CV_SAFE_RELEASE_MAT( XAxis );
		CV_SAFE_RELEASE_MAT( YAxis );
		CV_SAFE_RELEASE_MAT( ZAxis );
		CV_SAFE_RELEASE_MAT( RotNew );
		CV_SAFE_RELEASE_MAT( IntrinsicTmp );
		CV_SAFE_RELEASE_MAT( IntrinsicNew );
		CV_SAFE_RELEASE_MAT( tmpMat0 );
		CV_SAFE_RELEASE_MAT( tmpMat1 );
		CV_SAFE_RELEASE_MAT( tmpMat3 );
		CV_SAFE_RELEASE_MAT( tmpMat4 );
		CV_SAFE_RELEASE_MAT( tmpMat5 );

	}

	//implementation of [1]
	void Rectify_Simple2(/*IN*/const float* POld0, /*IN*/ const float* POld1, 
		/*OUT*/float* PNew0, /*OUT*/float* PNew1 )
	{
		printf("To be implemented!\n");
	}

}